package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.PIDSource;

/**
 *
 * @author buttercream
 */
public class AngularEncoder implements PIDSource
{
    private AnalogChannel raw;
    private int numTicks;
    private double angleOffset = 0.0;

    public AngularEncoder(int slot, int channel, int numTicks)
    {
        raw = new AnalogChannel(slot, channel);
        this.numTicks = numTicks;
    }

    /**
     * Overloaded method that gets called during the PID calculations
     * @return angle that the encoder is currently at
     */
    public double pidGet()
    {
        return calculateAngle();
    }

    public double getAngle()
    {
        return calculateAngle();
    }

    public int getRawValue()
    {
        return raw.getValue();
    }

    /**
     * Map the encoder value to [0,359]
     * @return Calculated angle
     */
    private double calculateAngle()
    {
        int rawVal = raw.getValue();

        if(rawVal > numTicks)
        {
            rawVal = numTicks;
        }

        double ratio = rawVal/numTicks;
        double retAngle = ratio*360 - angleOffset;

        return Utils.mod360(retAngle);
    }

    public void zeroAngle()
    {
        angleOffset = 0.0;
        angleOffset = calculateAngle();
    }

}
